// Copyright (c) 2022.
// Written by Li Yanxiao. All rights reserved

#include "draw.h"
#include <iostream>

using std::cout;
using std::endl;

void draw(cv::Mat &image, const Eigen::MatrixXd *distances) {
    // 验证待写入的图像与距离长度的大小一样
    assert(image.rows == distances->rows() && image.cols == distances->cols());
    auto normalized_matrix = new Eigen::MatrixXd(distances->rows(), distances->cols());
    normalize(normalized_matrix, distances);

    for (int i=0;i<image.rows;i++){
        for (int j=0;j <image.cols;j++){
            double distance = (*normalized_matrix)(i,j);
            if (distance >= 0) {
                image.at<cv::Vec3b>(i, j)[0] = 255 - floor(distance);
                image.at<cv::Vec3b>(i, j)[1] = 255 - floor(distance);
                image.at<cv::Vec3b>(i, j)[2] = 255;
            }
            else {
                image.at<cv::Vec3b>(i, j)[0] = 255;
                image.at<cv::Vec3b>(i, j)[1] = 255 + floor(distance);
                image.at<cv::Vec3b>(i, j)[2] = 255 + floor(distance);
            }
        }
    }
}

Eigen::MatrixXd *normalize(Eigen::MatrixXd *dst, const Eigen::MatrixXd *distances) {
    assert(dst != nullptr && distances != nullptr); // 空指针检测
    assert(dst->rows() == distances->rows() && dst->cols() == distances->cols()); // 检查两个矩阵大小是否相等
    Eigen::MatrixXd abs_matrix = distances->array().abs().matrix();
    double max_value = abs_matrix.maxCoeff();
    for (int i=0;i<distances->rows();i++){
        for (int j=0;j<distances->cols();j++){
            double val = (*distances)(i,j);
            (*dst)(i, j) = val / max_value * 255;
        }
    }
    return dst;
}

